/**
  ****************************(C) COPYRIGHT 2024 Polarbear****************************
  * @file       chassis_mecanum.c/h
  * @brief      麦轮轮底盘控制器。
  * @note       包括初始化，目标量更新、状态量更新、控制量计算与直接控制量的发送
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-9-2024      Tina_Lin         1. done
  *  V1.0.1     Dec-9-2024      Tina_Lin         1. 完成基本控制
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2024 Polarbear****************************
*/


#include "robot_param.h"
#if (CHASSIS_TYPE == CHASSIS_MECANUM_WHEEL)
#include "chassis_mecanum.h"
#include "CAN_receive.h"
#include "chassis.h"
#include "usb_task.h"
#include "motor.h"
#include "detect_task.h"
#include "gimbal.h"
#include "math.h"
#include "referee.h"
#include "usb_debug.h"
#include "ui.h"
    // 功率限制参数初始化
    uint16_t max_power_limit = 40;         // 初始最大功率限制值
    fp32 chassis_max_power = 0;            // 最终计算得到的底盘最大功率
    float input_power = 0;                 // 电容输入功率
    float initial_give_power[4];           // 各电机初始计算功率
    fp32 scaled_give_power[4];             // 缩放后的电机功率

    // 获取当前底盘功率数据
    fp32 chassis_power = 0.0f;             // 当前底盘功率
    fp32 chassis_power_buffer = 0.0f;      // 功率缓冲池值
    fp32 toque_coefficient = 1.99688994e-6f; // (20/16384)*(0.3)*(187/3591)/9.55
	fp32 k2 = 1.23e-07;						 // k1
	fp32 k1 = 1.453e-07;					 // k2
	fp32 constant = 4.081f;
    fp32 buffer_pid_out = 0.0f;             // 功率缓冲池PID输出值
    pid_type_def buffer_pid;                       // 功率缓冲池PID控制器
    bool cap_state = false;                 // 功率限制状态
    float initial_total_power = 0;
    uint8_t cap_percent = 0;                // 功率限制百分比

    float i_total_power = 0;
Chassis_s CHASSIS;
CHASSIS_PID_t chassis_pid;
bool spinflag = false; //标定小陀螺状态
bool followflag = false;//标定跟随状态
bool no_forceflag = true;//标定无力状态
fp32 speedflag = 0.0f;
ramp_function_source_t chassis_ramp_x,chassis_ramp_y;
ramp_function_source_t chassis_ramp_w1,chassis_ramp_w2,chassis_ramp_w3,chassis_ramp_w4;

extern int ui_self_id = 0;
/*-------------------- Init --------------------*/

/**
 * @brief          初始化
 * @param[in]      none
 * @retval         none
 */
void ChassisInit(void)
{
    CHASSIS.rc = get_remote_control_point();  // 获取遥控器指针
    /*-------------------- 初始化PID --------------------*/
    //速度环pid
    float chassis_speed_pid[3] = {KP_CHASSIS_WHEEL_SPEED, KI_CHASSIS_WHEEL_SPEED, KD_CHASSIS_WHEEL_SPEED};
    //yaw轴跟踪pid
    float chassis_yaw_pid[3] = {KP_CHASSIS_GIMBAL_FOLLOW_ANGLE, KI_CHASSIS_GIMBAL_FOLLOW_ANGLE, KD_CHASSIS_GIMBAL_FOLLOW_ANGLE};
    
    float power_limit_pid[3] = {
        KP_CHASSIS_POWER_LIMIT, KI_CHASSIS_POWER_LIMIT, KD_CHASSIS_POWER_LIMIT};

    //获取底盘电机数据指针，初始化PID 
    uint8_t i;
    for (i = 0; i < 4; i++)
    {
        //底盘不跟随云台下的pid初始化
        PID_init(&chassis_pid.motor_speed_pid[i], PID_POSITION, chassis_speed_pid, MAX_OUT_CHASSIS_WHEEL_SPEED,MAX_IOUT_CHASSIS_WHEEL_SPEED);
    }
    //底盘跟随云台pid初始化
        PID_init(&chassis_pid.chassis_angle_pid, PID_POSITION, chassis_yaw_pid, MAX_OUT_CHASSIS_GIMBAL_FOLLOW_ANGLE,MAX_IOUT_CHASSIS_GIMBAL_FOLLOW_ANGLE);
    
        PID_init(
            &buffer_pid, PID_POSITION, power_limit_pid,
            MAX_OUT_CHASSIS_POWER_LIMIT, MAX_IOUT_CHASSIS_POWER_LIMIT); 		// 功率限制PID初始化
    /*-------------------- 初始化底盘电机 --------------------*/
    MotorInit(&CHASSIS.wheel_motor[0], 1, 1, DJI_M3508, 1, 1, 1);
    MotorInit(&CHASSIS.wheel_motor[1], 2, 1, DJI_M3508, 1, 1, 1);
    MotorInit(&CHASSIS.wheel_motor[2], 3, 1, DJI_M3508, 1, 1, 1);
    MotorInit(&CHASSIS.wheel_motor[3], 4, 1, DJI_M3508, 1, 1, 1);
    ramp_init(&chassis_ramp_x, 0.01f, NORMAL_MAX_CHASSIS_SPEED_X, -NORMAL_MAX_CHASSIS_SPEED_X);
    ramp_init(&chassis_ramp_y, 0.01f, NORMAL_MAX_CHASSIS_SPEED_Y, -NORMAL_MAX_CHASSIS_SPEED_Y);
    ramp_init(&chassis_ramp_w1, 0.01f, NORMAL_MAX_CHASSIS_SPEED_WX_LV2, -NORMAL_MAX_CHASSIS_SPEED_WX_LV2);
    ramp_init(&chassis_ramp_w2, 0.01f, NORMAL_MAX_CHASSIS_SPEED_WX_LV4, -NORMAL_MAX_CHASSIS_SPEED_WX_LV4);
    ramp_init(&chassis_ramp_w3, 0.01f, NORMAL_MAX_CHASSIS_SPEED_WX_LV6, -NORMAL_MAX_CHASSIS_SPEED_WX_LV6);
    ramp_init(&chassis_ramp_w4, 0.01f, NORMAL_MAX_CHASSIS_SPEED_WX_LV8, -NORMAL_MAX_CHASSIS_SPEED_WX_LV8);
}

/*-------------------- Set mode --------------------*/

/**
 * @brief          设置模式
 * @param[in]      none
 * @retval         none
 */
void ChassisSetMode(void)
{
    CHASSIS.last_mode = CHASSIS.mode;
    if (switch_is_up(CHASSIS.rc->rc.s[CHASSIS_MODE_CHANNEL])||spinflag) {
        CHASSIS.mode = CHASSIS_SPIN;
    // } else if (switch_is_mid(CHASSIS.rc->rc.s[CHASSIS_MODE_CHANNEL]) && (GetGimbalInitJudgeReturn())) {
    //     CHASSIS.mode = CHASSIS_FREE;
    } else if (switch_is_mid(CHASSIS.rc->rc.s[CHASSIS_MODE_CHANNEL]) && (GetGimbalInitJudgeReturn())||followflag) {
        CHASSIS.mode = CHASSIS_FOLLOW_GIMBAL_YAW;
    } else if (switch_is_down(CHASSIS.rc->rc.s[CHASSIS_MODE_CHANNEL]) || !(GetGimbalInitJudgeReturn())||no_forceflag) {
        CHASSIS.mode = CHASSIS_ZERO_FORCE;
    }
}


/*-------------------- Observe --------------------*/

/**
 * @brief          更新状态量
 * @param[in]      none
 * @retval         none
 */
void ChassisObserver(void) {
    for (uint8_t i = 0; i < 4; i++) {
        GetMotorMeasure(&CHASSIS.wheel_motor[i]);
    }
    //键鼠确定的状态值
    speedflag = 1.0f;
    if (CHASSIS.rc->key.v & KEY_PRESSED_OFFSET_R)//按R键启用小陀螺
    {
        spinflag = true;
        followflag = false;
        no_forceflag = false;
    }
    else if(CHASSIS.rc->key.v & KEY_PRESSED_OFFSET_F)//按F键开启底盘跟随
    {
        spinflag = false;
        followflag = true;
        no_forceflag = false;
    }
    else if(CHASSIS.rc->key.v & KEY_PRESSED_OFFSET_B)//按B键进入无力状态
    {
        spinflag = false;
        followflag = false;
        no_forceflag = true;
    }

    if(CHASSIS.rc->key.v & KEY_PRESSED_OFFSET_SHIFT)//shift加速
    {
        speedflag = speedflag*2;
    }
    if(CHASSIS.rc->key.v & KEY_PRESSED_OFFSET_V)//按B键进入无力状态
    {
        ui_init_g_Ungroup();
    }
         // 功率限制
    //功控数据
    get_chassis_power_and_buffer(&CHASSIS.chassis_power_limit ,&CHASSIS.chassis_power, &CHASSIS.buffer_energy,&CHASSIS.level,&CHASSIS.id);
    ui_self_id=CHASSIS.id;
}

/*-------------------- Reference --------------------*/

/**
 * @brief          更新目标量
 * @param[in]      none
 * @retval         none
 */
void ChassisReference(void) {

    fp32 rc_x, rc_y;
    fp32 vx_channel, vy_channel;
    rc_deadband_limit(CHASSIS.rc->rc.ch[3], rc_x, CHASSIS_RC_DEADLINE);
    rc_deadband_limit(CHASSIS.rc->rc.ch[2], rc_y, CHASSIS_RC_DEADLINE);

    vx_channel = rc_x * MAX_SPEED_VECTOR_VX;
    vy_channel = rc_y * MAX_SPEED_VECTOR_VY;

    CHASSIS.dyaw = GetGimbalDeltaYawMid();

    CHASSIS.vx_rc_set = vx_channel;
    CHASSIS.vy_rc_set = -vy_channel;

    // //键盘控制

    if (CHASSIS.rc->key.v & KEY_PRESSED_OFFSET_W)
    {
        ramp_calc(&chassis_ramp_x, NORMAL_MAX_CHASSIS_SPEED_X);
        CHASSIS.vx_rc_set = speedflag*chassis_ramp_x.out;
    }
    else if (CHASSIS.rc->key.v & KEY_PRESSED_OFFSET_S)
    {
        ramp_calc(&chassis_ramp_x, (-NORMAL_MAX_CHASSIS_SPEED_X));
        CHASSIS.vx_rc_set = speedflag*chassis_ramp_x.out;
    }
    else
    {
        chassis_ramp_x.out = 0.0f;
    }

    if (CHASSIS.rc->key.v & KEY_PRESSED_OFFSET_A) 
    {
        ramp_calc(&chassis_ramp_y, NORMAL_MAX_CHASSIS_SPEED_Y);
        CHASSIS.vy_rc_set = speedflag*chassis_ramp_y.out;
    }
    else if (CHASSIS.rc->key.v & KEY_PRESSED_OFFSET_D)
    {
        ramp_calc(&chassis_ramp_y, (-NORMAL_MAX_CHASSIS_SPEED_Y));
        CHASSIS.vy_rc_set = speedflag*chassis_ramp_y.out;
    }
    else
    {
        chassis_ramp_y.out = 0.0f;
    }

    //具体模式设定
    switch (CHASSIS.mode) {
        case CHASSIS_ZERO_FORCE: { 
            CHASSIS.wz_set = CHASSIA_STOP_SPEED;
            CHASSIS.vx_set = CHASSIA_STOP_SPEED;
            CHASSIS.vy_set = CHASSIA_STOP_SPEED;
            break;
        }
        case CHASSIS_FOLLOW_GIMBAL_YAW:{//云台跟随模式

            fp32 sin_yaw = 0.0f, cos_yaw = 0.0f;
	        // 控制vx vy
	        sin_yaw = sinf(CHASSIS.dyaw);
	        cos_yaw = cosf(CHASSIS.dyaw);
            CHASSIS.vy_set = -cos_yaw * CHASSIS.vy_rc_set + sin_yaw * CHASSIS.vx_rc_set;
	        CHASSIS.vx_set = sin_yaw * CHASSIS.vy_rc_set + cos_yaw * CHASSIS.vx_rc_set; 
            CHASSIS.wz_set = PID_calc(&chassis_pid.chassis_angle_pid, CHASSIS.dyaw, 0);//反转dyaw角度
            if(CHASSIS.dyaw>0.3f)
            {
                CHASSIS.wz_set = -NORMAL_MAX_CHASSIS_SPEED_WX_LV4;
            }
            else if(CHASSIS.dyaw<-0.3f)
            {
                CHASSIS.wz_set = NORMAL_MAX_CHASSIS_SPEED_WX_LV4;
            }
            break;
        }
        case CHASSIS_STOP:
            break;
        case CHASSIS_FREE:{//底盘不跟随云台
            CHASSIS.wz_set = NORMAL_MIN_CHASSIS_SPEED_WX;//暂时让这个模式下小陀螺不生效
            CHASSIS.vy_set = -CHASSIS.vy_rc_set;
	        CHASSIS.vx_set = CHASSIS.vx_rc_set;
            break;
        }
        case CHASSIS_SPIN:{//小陀螺模式
            if(CHASSIS.last_mode!=CHASSIS_SPIN)
            {
                chassis_ramp_w1.out = 0.0f;
                chassis_ramp_w2.out = 0.0f;
                chassis_ramp_w3.out = 0.0f;
                chassis_ramp_w4.out = 0.0f;
            }
            fp32 sin_yaw = 0.0f, cos_yaw = 0.0f;
	        // 控制vx vy
	        sin_yaw = sinf(CHASSIS.dyaw);
	        cos_yaw = cosf(CHASSIS.dyaw);
            CHASSIS.vy_set = -cos_yaw * CHASSIS.vy_rc_set + sin_yaw * CHASSIS.vx_rc_set;
	        CHASSIS.vx_set = sin_yaw * CHASSIS.vy_rc_set + cos_yaw * CHASSIS.vx_rc_set;
            if(CHASSIS.level<=2)
            {
                ramp_calc(&chassis_ramp_w1, NORMAL_MAX_CHASSIS_SPEED_WX_LV2);
                CHASSIS.wz_set = speedflag*chassis_ramp_w1.out;
            }
            else if (CHASSIS.level>2&&CHASSIS.level<=4)
            {
                ramp_calc(&chassis_ramp_w2, NORMAL_MAX_CHASSIS_SPEED_WX_LV4);
                CHASSIS.wz_set = speedflag*chassis_ramp_w2.out;
            }
            else if (CHASSIS.level>4&&CHASSIS.level<=6)
            {
                ramp_calc(&chassis_ramp_w3, NORMAL_MAX_CHASSIS_SPEED_WX_LV6);
                CHASSIS.wz_set = speedflag*chassis_ramp_w3.out;
            }
            else
            {
                ramp_calc(&chassis_ramp_w4, NORMAL_MAX_CHASSIS_SPEED_WX_LV8);
                CHASSIS.wz_set = speedflag*chassis_ramp_w4.out;
            }
            break;
        }
        case CHASSIS_AUTO:
            break;
        case CHASSIS_OPEN: {
            uint16_t current;
            uint8_t i;
            current = CHASSIS.rc->rc.ch[CHASSIS_X_CHANNEL] * 3000 * RC_TO_ONE;
            for (i = 0; i < 4; i++) {
                CHASSIS.wheel_motor[0].set.curr = current;
            }
            break;
        }
        default:
            break;
    }
}

/*-------------------- Console --------------------*/

/**
 * @brief          计算控制量
 * @param[in]      none
 * @retval         none
 */
void ChassisConsole(void)
{
    float initial_total_power = 0;         // 电机总功率
    uint8_t i;

    // 判断是否出错，若出错则将电流全部置零
    if (toe_is_error(DBUS_TOE))
    {
        for (i = 0; i < 4; i++)
        {
            CHASSIS.wheel_motor[i].set.curr = CHASSIA_CURR_ZERO; 
        }
        return;
    }
    
    //麦轮解算
    CHASSIS.wheel_motor[0].set.vel = CHASSIS.vx_set + CHASSIS.vy_set + (CHASSIS_WZ_SET_SCALE - 1.0f) * CHASSIS.wz_set;
    CHASSIS.wheel_motor[1].set.vel = CHASSIS.vx_set - CHASSIS.vy_set + (CHASSIS_WZ_SET_SCALE - 1.0f) * CHASSIS.wz_set;
    CHASSIS.wheel_motor[2].set.vel = -CHASSIS.vx_set + CHASSIS.vy_set + (CHASSIS_WZ_SET_SCALE - 1.0f) * CHASSIS.wz_set;
    CHASSIS.wheel_motor[3].set.vel = -CHASSIS.vx_set - CHASSIS.vy_set + (CHASSIS_WZ_SET_SCALE - 1.0f) * CHASSIS.wz_set; 


    //pid速度计算            
    for (i = 0; i < 4; i++)
    {
        CHASSIS.wheel_motor[i].set.curr =
        PID_calc(&chassis_pid.motor_speed_pid[i], CHASSIS.wheel_motor[i].fdb.vel, CHASSIS.wheel_motor[i].set.vel);
    }

        // 功率限制
     // 缓冲池PID计算（目标值30）
     buffer_pid_out = PID_calc(&buffer_pid, CHASSIS.buffer_energy,50);

     // 基于缓冲池PID输出的输入功率计算（在最大功率附近浮动）
     input_power = CHASSIS.chassis_power_limit - buffer_pid_out;

      /* 根据电容状态计算最大输出功率 */
    // if (cap_percent > 5) {
    //     // 电容充足时根据状态增加可用功率
    //     chassis_max_power = (cap_state == 0) ? (input_power + 5) : (input_power + 200);
    // } else {
    //     chassis_max_power = input_power;  // 电容不足时直接使用输入功率
    // }

    chassis_max_power = input_power;

     // 计算各电机初始功率（包含电机损耗模型）
    for (uint8_t i = 0; i < 4; i++) {
        /* 功率计算公式组成：
           - 扭矩相关项：PID输出 * 扭矩系数 * 转速 
           - 转速平方项：k2 * 转速^2
           - PID输出平方项：a * PID输出^2
           - 恒定损耗项：constant */
        // initial_give_power[i] = CHASSIS.wheel_motor[i].set.curr * toque_coefficient * CHASSIS.wheel_motor[i].fdb.vel/RPM_TO_OMEGA+   //此处需要rpm单位转速，还没有进行单位转换
        //                       k1 * CHASSIS.wheel_motor[i].fdb.vel/RPM_TO_OMEGA * CHASSIS.wheel_motor[i].fdb.vel/RPM_TO_OMEGA +
        //                       k2 * CHASSIS.wheel_motor[i].set.curr * CHASSIS.wheel_motor[i].set.curr + constant;
        initial_give_power[i] = CHASSIS.wheel_motor[i].set.curr * toque_coefficient * CHASSIS.wheel_motor[i].fdb.vel +   //此处需要rpm单位转速，还没有进行单位转换
        k1 * CHASSIS.wheel_motor[i].fdb.vel * CHASSIS.wheel_motor[i].fdb.vel +
        k2 * CHASSIS.wheel_motor[i].set.curr * CHASSIS.wheel_motor[i].set.curr + constant;

        if (initial_give_power[i] < 0) continue; // 忽略瞬态负功率
        initial_total_power += initial_give_power[i];
        i_total_power = initial_total_power ;
    }

    // 总功率超限时进行缩放分配
    if (initial_total_power > chassis_max_power) {
        fp32 power_scale = chassis_max_power / initial_total_power; // 功率缩放系数
        for (uint8_t i = 0; i < 4; i++) {
            
            scaled_give_power[i] = initial_give_power[i] * power_scale;

            if (scaled_give_power[i] < 0) continue;
        
            fp32 a = k2;
            fp32 b = toque_coefficient * CHASSIS.wheel_motor[i].fdb.vel/RPM_TO_OMEGA; //此处需要rpm单位转速，还没有进行单位转换
            fp32 c = k1 * CHASSIS.wheel_motor[i].fdb.vel/RPM_TO_OMEGA * CHASSIS.wheel_motor[i].fdb.vel/RPM_TO_OMEGA - scaled_give_power[i] + constant;
            fp32 inside = b * b - 4 * a * c; // 判别式计算

            if (inside < 0) continue;//无实数解则保留原输出

            /* 根据原输出方向选择解并限幅（±16000）*/
            if (CHASSIS.wheel_motor[i].set.curr > 0) {
                fp32 temp = (-b + sqrt(inside)) / (2 * a);
                CHASSIS.wheel_motor[i].set.curr = (temp > 16000) ? 16000 : temp;} 
            else {
                fp32 temp = (-b - sqrt(inside)) / (2 * a);
                CHASSIS.wheel_motor[i].set.curr = (temp < -16000) ? -16000 : temp;
            }
        }
    }
}

/*-------------------- Cmd --------------------*/

/**
 * @brief          发送控制量
 * @param[in]      none
 * @retval         none
 */

void ChassisSendCmd(void)
{
    CanCmdDjiMotor(1, 0x200, 
    CHASSIS.wheel_motor[0].set.curr, CHASSIS.wheel_motor[1].set.curr,
    CHASSIS.wheel_motor[2].set.curr, CHASSIS.wheel_motor[3].set.curr);

    // ModifyDebugDataPackage(1, input_power, "inputp");
    // ModifyDebugDataPackage(2, chassis_max_power / i_total_power, "xishu");
    // ModifyDebugDataPackage(3, CHASSIS.buffer_energy, "buf_fdb"); 
    // ModifyDebugDataPackage(4, initial_total_power, "initial_total_power");
    // ModifyDebugDataPackage(5, buffer_pid_out, "buffer_pid_out");
}

#endif
